package main

import (
	"math"
	"strconv"
)

const (
	G_GMEARTH      float64 = 3.986005e14
	G_EARTH_ROTATE float64 = 7.2921151467e-5

	DEG2RAD float64 = math.Pi / 180.0
	RAD2DEG float64 = 180.0 / math.Pi

	DAY_SECS  int = 86400  // 60 * 60 * 24;
	WEEK_SECS int = 604800 //86400 * 7; //

	SYS_UNKOWN = 0
	SYS_BDS    = 1
	SYS_GPS    = 2
	SYS_GLO    = 3
	SYS_GAL    = 4
	SYS_QZS    = 5 // QZSS
)

type GnssEPH struct {
	Prn  byte
	Sys  byte
	CPrn string

	Week   int
	TOE    int
	Omega0 float64 // TOE时刻的Omega
	SOD    int
	MJD    int
	//A0, A1, A2 float64
	RootA    float64
	DeltaN   float64
	M0       float64
	Cuc, Cus float64
	Crc, Crs float64
	Cic, Cis float64
	E        float64 // 卫星轨道的偏心率

	Omega    float64 // 近地点角距
	Omegadot float64
	I0       float64
	Idot     float64
}

func NewGnssEPH() *GnssEPH {
	rval := &GnssEPH{}
	return rval
}

func (this *GnssEPH) SetCPrn(val string) {
	if len(val) == 3 {
		switch val[0] {
		case 'C':
			this.Sys = SYS_BDS
		case 'G':
			this.Sys = SYS_GPS
		case 'E':
			this.Sys = SYS_GAL
		case 'R':
			this.Sys = SYS_GLO
		case 'J':
			this.Sys = SYS_QZS
		default:
			this.Sys = 0
		}
	}
	v, _ := strconv.ParseInt(val[1:], 10, 8)
	this.Prn = byte(v)
}

func CalcuEphXyz(eph *GnssEPH, wk, sow int) (x, y, z float64) {

	var GMEARTH, EARTH_ROTATE float64
	switch eph.Sys {
	case SYS_GPS:
		GMEARTH = 3.986005e14
		EARTH_ROTATE = 7.2921151467e-5
		//break;
	case SYS_GAL:
		GMEARTH = 3.986004418e14
		EARTH_ROTATE = 7.2921151467e-5
		//break;
	case SYS_BDS:
		GMEARTH = 3.986004418e14
		EARTH_ROTATE = 7.2921150e-5
		//break;
	case SYS_QZS:
		GMEARTH = 3.986005e14
		EARTH_ROTATE = 7.2921150e-5
		//break;
	default:
		GMEARTH = 3.986004415e14       //TT-compatible;
		EARTH_ROTATE = 7.2921151467e-5 // 地球自转角速度 rad/s
		//break;
	}

	dt := float64((wk-eph.Week)*WEEK_SECS + sow - eph.TOE)
	if dt > 302400 { // 半周(秒)
		dt = dt - 604800 // -一周秒
	} else if dt < -302400 {
		dt += 604800
	}

	a := eph.RootA * eph.RootA           // 轨道长半轴
	xn := math.Sqrt(GMEARTH / a / a / a) //TOE 参考时刻的平均角速度 (n0)
	xn += eph.DeltaN                     // 观测时刻平均角速度  对平均运动角速度进行改正
	xm := eph.M0 + xn*float64(dt)        // 观察瞬间 卫星的平近点角
	ex := xm
	e := eph.E

	for i := 0; i < 12; i++ {
		ex = xm + e*math.Sin(ex) // 计算偏近点角
	}

	// 计算真近点角
	v0 := 1.0 - e*math.Cos(ex)
	vs := math.Sqrt(1.0-e*e) * math.Sin(ex) / v0 // sinf
	vc := (math.Cos(ex) - e) / v0                // cosf
	v := math.Abs(math.Asin(vs))

	if vc >= 0 {
		if vs < 0 {
			v = 2.0*math.Pi - v
		}
	} else {
		if vs <= 0 {
			v = math.Pi + v
		} else {
			v = math.Pi - v
		}
	}

	phi := v + eph.Omega // 升交角距
	c2u := math.Cos(2.0 * phi)
	s2u := math.Sin(2.0 * phi)
	du := eph.Cuc*c2u + eph.Cus*s2u // 升交角距u的改动项
	dr := eph.Crc*c2u + eph.Crs*s2u // 卫星矢径r的改正项
	di := eph.Cic*c2u + eph.Cis*s2u // 卫星轨道倾角i的摄动改正项

	// 进行摄动改正
	xu := phi + du
	xr := a*(1.0-e*math.Cos(ex)) + dr
	xi := eph.I0 + eph.Idot*float64(dt) + di

	xx := xr * math.Cos(xu)
	yy := xr * math.Sin(xu)

	var xnode float64
	if eph.Sys == SYS_BDS && (eph.Prn == 1 || eph.Prn == 2 || eph.Prn == 3 || eph.Prn == 4 || eph.Prn == 5) {
		xnode = eph.Omega0 + eph.Omegadot*float64(dt)
	} else {
		xnode = eph.Omega0 + (eph.Omegadot-EARTH_ROTATE)*float64(dt)
	}
	xnode = xnode - EARTH_ROTATE*float64(eph.TOE)
	x = xx*math.Cos(xnode) - yy*math.Cos(xi)*math.Sin(xnode)
	y = xx*math.Sin(xnode) + yy*math.Cos(xi)*math.Cos(xnode)
	z = yy * math.Sin(xi)

	if eph.Sys == SYS_BDS && (eph.Prn == 1 || eph.Prn == 2 || eph.Prn == 3 || eph.Prn == 4 || eph.Prn == 5) {
		x1 := x
		y1 := math.Cos(-5.0*DEG2RAD)*y + math.Sin(-5.0*DEG2RAD)*z
		z1 := math.Sin(5.0*DEG2RAD)*y + math.Cos(-5.0*DEG2RAD)*z

		x = x1*math.Cos(EARTH_ROTATE*dt) + y1*math.Sin(EARTH_ROTATE*dt)
		y = -1.0*x1*math.Sin(EARTH_ROTATE*dt) + y1*math.Cos(EARTH_ROTATE*dt)
		z = z1
	}

	return
}
